using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using static LightCAD.MathLib.MathEx;

namespace LightCAD.MathLib
{
    public sealed class Euler
    {
        public enum RotationOrders
        {
            None,
            XYZ,
            YZX,
            ZXY,
            XZY,
            YXZ,
            ZYX
        }
        #region scope properties or methods
        //private static Matrix4 _matrix = new Matrix4();
        //private static Quaternion _quaternion = new Quaternion();
        private static EulerContext GetContext()
        {
            return ThreadContext.GetCurrThreadContext().EulerCtx;
        }
        #endregion

        #region Properties

        public const RotationOrders DefaultOrder = RotationOrders.XYZ;

        internal double _x;
        internal double _y;
        internal double _z;
        internal RotationOrders _order;

        private Action onChangeCallbackAction;

        #endregion

        #region constructor
        public Euler(double x = 0, double y = 0, double z = 0, RotationOrders order = Euler.DefaultOrder)
        {
            this._x = x;
            this._y = y;
            this._z = z;
            this._order = order;
        }
        public Euler(double x, double y, double z, string orderStr = "XYZ")
        {
            this._x = x;
            this._y = y;
            this._z = z;
            switch (orderStr)
            {
                case "YZX":
                    this._order = RotationOrders.YZX;
                    break;
                case "ZXY":
                    this._order = RotationOrders.ZXY;
                    break;
                case "XZY":
                    this._order = RotationOrders.XZY;
                    break;
                case "YXZ":
                    this._order = RotationOrders.YXZ;
                    break;
                case "ZYX":
                    this._order = RotationOrders.ZYX;
                    break;
                default:
                    this._order = Euler.DefaultOrder;
                    break;
            }
        }
        #endregion

        #region properties
        public double X
        {
            get
            {
                return this._x;
            }
            set
            {
                this._x = value;
                this.OnChangeCallback();
            }
        }
        public double Y
        {
            get
            {
                return this._y;
            }
            set
            {
                this._y = value;
                this.OnChangeCallback();
            }
        }
        public double Z
        {
            get
            {
                return this._z;
            }
            set
            {
                this._z = value;
                this.OnChangeCallback();
            }
        }
        public RotationOrders Order
        {
            get
            {
                return this._order;
            }
            set
            {
                this._order = value;
                this.OnChangeCallback();
            }
        }
        #endregion

        #region methods
        [MethodImpl((MethodImplOptions)768)]
        public Euler Set(double x, double y, double z, RotationOrders order = RotationOrders.None)
        {
            this._x = x;
            this._y = y;
            this._z = z;
            if (order != RotationOrders.None)
                this._order = order;
            this.OnChangeCallback();
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler Clone()
        {
            return new Euler(this._x, this._y, this._z, this._order);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler Copy(Euler euler)
        {
            this._x = euler._x;
            this._y = euler._y;
            this._z = euler._z;
            this._order = euler._order;
            this.OnChangeCallback();
            return this;
        }
        public Euler SetFromRotationMatrix(Matrix4 m, RotationOrders order = RotationOrders.None, bool update = true)
        {
            // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
            double[] te = m.Elements;
            double m11 = te[0], m12 = te[4], m13 = te[8];
            double m21 = te[1], m22 = te[5], m23 = te[9];
            double m31 = te[2], m32 = te[6], m33 = te[10];
            if (order == RotationOrders.None)
                order = this._order;

            switch (order)
            {
                case RotationOrders.XYZ:
                    this._y = Math.Asin(Clamp(m13, -1, 1));
                    if (Math.Abs(m13) < 0.9999999)
                    {
                        this._x = Math.Atan2(-m23, m33);
                        this._z = Math.Atan2(-m12, m11);
                    }
                    else
                    {
                        this._x = Math.Atan2(m32, m22);
                        this._z = 0;
                    }
                    break;
                case RotationOrders.YXZ:
                    this._x = Math.Asin(-Clamp(m23, -1, 1));
                    if (Math.Abs(m23) < 0.9999999)
                    {
                        this._y = Math.Atan2(m13, m33);
                        this._z = Math.Atan2(m21, m22);
                    }
                    else
                    {
                        this._y = Math.Atan2(-m31, m11);
                        this._z = 0;
                    }
                    break;
                case RotationOrders.ZXY:
                    this._x = Math.Asin(Clamp(m32, -1, 1));
                    if (Math.Abs(m32) < 0.9999999)
                    {
                        this._y = Math.Atan2(-m31, m33);
                        this._z = Math.Atan2(-m12, m22);
                    }
                    else
                    {
                        this._y = 0;
                        this._z = Math.Atan2(m21, m11);
                    }
                    break;
                case RotationOrders.ZYX:
                    this._y = Math.Asin(-Clamp(m31, -1, 1));
                    if (Math.Abs(m31) < 0.9999999)
                    {
                        this._x = Math.Atan2(m32, m33);
                        this._z = Math.Atan2(m21, m11);
                    }
                    else
                    {
                        this._x = 0;
                        this._z = Math.Atan2(-m12, m22);
                    }
                    break;
                case RotationOrders.YZX:
                    this._z = Math.Asin(Clamp(m21, -1, 1));
                    if (Math.Abs(m21) < 0.9999999)
                    {
                        this._x = Math.Atan2(-m23, m22);
                        this._y = Math.Atan2(-m31, m11);
                    }
                    else
                    {
                        this._x = 0;
                        this._y = Math.Atan2(m13, m33);
                    }
                    break;
                case RotationOrders.XZY:
                    this._z = Math.Asin(-Clamp(m12, -1, 1));
                    if (Math.Abs(m12) < 0.9999999)
                    {
                        this._x = Math.Atan2(m32, m22);
                        this._y = Math.Atan2(m13, m11);
                    }
                    else
                    {
                        this._x = Math.Atan2(-m23, m33);
                        this._y = 0;
                    }
                    break;
            }
            this._order = order;
            if (update) this.OnChangeCallback();
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler SetFromQuaternion(Quaternion q, RotationOrders order, bool update = true)
        {
            var _matrix = GetContext()._matrix;
            _matrix.MakeRotationFromQuaternion(q);
            return this.SetFromRotationMatrix(_matrix, order, update);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler SetFromVector3(Vector3 v, RotationOrders order = RotationOrders.None)
        {
            return this.Set(v.X, v.Y, v.Z, order);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler Reorder(RotationOrders newOrder)
        {
            // WARNING: this discards revolution information -bhouston
            var _quaternion = GetContext()._quaternion;
            _quaternion.SetFromEuler(this);
            return this.SetFromQuaternion(_quaternion, newOrder);
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(Euler euler)
        {
            return (euler._x == this._x) && (euler._y == this._y) && (euler._z == this._z) && (euler._order == this._order);
        }
        public Euler FromArray(double[] array)
        {
            this._x = array[0];
            this._y = array[1];
            this._z = array[2];
            if (array.Length == 4)
            {
                this._order = (RotationOrders)(int)array[3];
            }
            this.OnChangeCallback();
            return this;
        }
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[4];
            array[offset] = this._x;
            array[offset + 1] = this._y;
            array[offset + 2] = this._z;
            array[offset + 3] = (double)this._order;
            return array;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Euler OnChange(Action callback)
        {
            this.onChangeCallbackAction = callback;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public void OnChangeCallback()
        {
            onChangeCallbackAction?.Invoke();
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ToVector3(Vector3 target)
        {
            return target.SetFromEuler(this);
        }
        #endregion

        public IEnumerator GetEnumerator()
        {
            yield return this._x;
            yield return this._y;
            yield return this._z;
            yield return this._order;
        }
    }
    public sealed class EulerContext
    {
        public readonly Matrix4 _matrix = new Matrix4();
        public readonly Quaternion _quaternion = new Quaternion();
    }
}
